/**
 * @file test_scheduled_work_item.cpp
 * @brief Unit tests for ScheduledWorkItem
 */

#include <px4_platform_common/px4_work_queue/scheduled_work_item.hpp>
#include <px4_platform_common/px4_work_queue/work_queue_manager.hpp>
#include "FreeRTOS.h"
#include "task.h"

// Forward declaration for debug_printf
extern "C" int debug_printf(const char *format, ...);

using namespace px4;

// Test work item class
class TestScheduledWorkItem : public ScheduledWorkItem
{
public:
	TestScheduledWorkItem(const char *name, const wq_config_t &config)
		: ScheduledWorkItem(name, config) {}

	uint32_t run_count{0};
	hrt_abstime last_run_time{0};

protected:
	void Run() override
	{
		run_count++;
		last_run_time = hrt_absolute_time();
	}
};

extern "C" void test_scheduled_work_item(void)
{
	debug_printf("\n========================================\n");
	debug_printf("  ScheduledWorkItem Tests\n");
	debug_printf("========================================\n");

	// Start WorkQueueManager
	WorkQueueManagerStart();

	uint32_t total_tests = 0;
	uint32_t passed_tests = 0;

	// Test 1: Basic construction and Scheduled() check
	{
		debug_printf("Test 1: Basic construction and Scheduled() check\n");

		TestScheduledWorkItem item("test_item1", px4::wq_configurations::test1);

		total_tests++;
		if (!item.Scheduled()) {
			debug_printf("  ✓ Item not scheduled initially\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should not be scheduled initially\n");
		}
	}

	// Test 2: ScheduleDelayed
	{
		debug_printf("Test 2: ScheduleDelayed\n");

		TestScheduledWorkItem item("test_item2", px4::wq_configurations::test1);

		hrt_abstime start_time = hrt_absolute_time();
		item.ScheduleDelayed(50000);  // 50ms delay

		total_tests++;
		if (item.Scheduled()) {
			debug_printf("  ✓ Item scheduled after ScheduleDelayed\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should be scheduled\n");
		}

		// Wait for execution
		vTaskDelay(pdMS_TO_TICKS(100));

		total_tests++;
		if (item.run_count == 1) {
			debug_printf("  ✓ Item ran once (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should have run once (count=%lu)\n", item.run_count);
		}

		hrt_abstime elapsed = item.last_run_time - start_time;
		total_tests++;
		if (elapsed >= 45000 && elapsed <= 70000) {  // 45-70ms tolerance
			debug_printf("  ✓ Delay timing correct (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ Delay timing incorrect (%llu us, expected ~50000 us)\n", elapsed);
		}

		total_tests++;
		if (!item.Scheduled()) {
			debug_printf("  ✓ Item not scheduled after one-shot execution\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should not be scheduled after one-shot\n");
		}
	}

	// Test 3: ScheduleAt
	{
		debug_printf("Test 3: ScheduleAt\n");

		TestScheduledWorkItem item("test_item3", px4::wq_configurations::test1);

		hrt_abstime target_time = hrt_absolute_time() + 50000;  // 50ms from now
		hrt_abstime start_time = hrt_absolute_time();
		item.ScheduleAt(target_time);

		// Wait for execution
		vTaskDelay(pdMS_TO_TICKS(100));

		total_tests++;
		if (item.run_count == 1) {
			debug_printf("  ✓ Item ran once (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should have run once (count=%lu)\n", item.run_count);
		}

		hrt_abstime elapsed = item.last_run_time - start_time;
		total_tests++;
		if (elapsed >= 45000 && elapsed <= 70000) {
			debug_printf("  ✓ Absolute time scheduling correct (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ Absolute time scheduling incorrect (%llu us)\n", elapsed);
		}
	}

	// Test 4: ScheduleOnInterval (periodic)
	{
		debug_printf("Test 4: ScheduleOnInterval (periodic)\n");

		TestScheduledWorkItem item("test_item4", px4::wq_configurations::test1);

		item.ScheduleOnInterval(30000, 10000);  // 30ms interval, 10ms initial delay

		total_tests++;
		if (item.Scheduled()) {
			debug_printf("  ✓ Item scheduled after ScheduleOnInterval\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should be scheduled\n");
		}

		// Wait for multiple executions
		vTaskDelay(pdMS_TO_TICKS(150));

		total_tests++;
		if (item.run_count >= 3 && item.run_count <= 6) {
			debug_printf("  ✓ Item ran multiple times (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item run count unexpected (count=%lu, expected 3-6)\n", item.run_count);
		}

		total_tests++;
		if (item.Scheduled()) {
			debug_printf("  ✓ Item still scheduled (periodic)\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should still be scheduled (periodic)\n");
		}

		// Clear the periodic schedule
		item.ScheduleClear();

		total_tests++;
		if (!item.Scheduled()) {
			debug_printf("  ✓ Item not scheduled after ScheduleClear\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should not be scheduled after ScheduleClear\n");
		}

		uint32_t count_before_clear = item.run_count;
		vTaskDelay(pdMS_TO_TICKS(100));

		total_tests++;
		if (item.run_count == count_before_clear) {
			debug_printf("  ✓ No more runs after ScheduleClear (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should not run after ScheduleClear (count=%lu)\n", item.run_count);
		}
	}

	// Test 5: ScheduleClear before execution
	{
		debug_printf("Test 5: ScheduleClear before execution\n");

		TestScheduledWorkItem item("test_item5", px4::wq_configurations::test1);

		item.ScheduleDelayed(100000);  // 100ms delay
		item.ScheduleClear();  // Cancel immediately

		vTaskDelay(pdMS_TO_TICKS(150));

		total_tests++;
		if (item.run_count == 0) {
			debug_printf("  ✓ Item did not run after ScheduleClear (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should not have run (count=%lu)\n", item.run_count);
		}
	}

	// Test 6: Reschedule
	{
		debug_printf("Test 6: Reschedule\n");

		TestScheduledWorkItem item("test_item6", px4::wq_configurations::test1);

		item.ScheduleDelayed(100000);  // 100ms delay
		vTaskDelay(pdMS_TO_TICKS(10));  // Wait a bit
		item.ScheduleDelayed(30000);  // Reschedule with 30ms delay

		hrt_abstime start_time = hrt_absolute_time();
		vTaskDelay(pdMS_TO_TICKS(80));

		total_tests++;
		if (item.run_count == 1) {
			debug_printf("  ✓ Item ran once after reschedule (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should have run once (count=%lu)\n", item.run_count);
		}

		hrt_abstime elapsed = item.last_run_time - start_time;
		total_tests++;
		if (elapsed >= 25000 && elapsed <= 50000) {
			debug_printf("  ✓ Reschedule timing correct (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ Reschedule timing incorrect (%llu us, expected ~30000 us)\n", elapsed);
		}
	}

	// Test 7: Statistics and print_run_status
	{
		debug_printf("Test 7: Statistics and print_run_status\n");

		TestScheduledWorkItem item("test_item7", px4::wq_configurations::test1);

		item.ScheduleOnInterval(25000, 0);  // 25ms interval, no initial delay
		vTaskDelay(pdMS_TO_TICKS(150));

		total_tests++;
		if (item.run_count >= 4) {
			debug_printf("  ✓ Item ran multiple times (count=%lu)\n", item.run_count);
			passed_tests++;
		} else {
			debug_printf("  ✗ Item should have run at least 4 times (count=%lu)\n", item.run_count);
		}

		debug_printf("  Status output:\n  ");
		item.print_run_status();

		item.ScheduleClear();
	}

	debug_printf("========================================\n");
	debug_printf("  Test Summary\n");
	debug_printf("========================================\n");
	debug_printf("Total tests: %lu\n", total_tests);
	debug_printf("Passed: %lu\n", passed_tests);
	debug_printf("Failed: %lu\n", total_tests - passed_tests);
	debug_printf("\n");

	if (passed_tests == total_tests) {
		debug_printf("✅ All ScheduledWorkItem tests PASSED!\n");
	} else {
		debug_printf("❌ Some ScheduledWorkItem tests FAILED!\n");
	}
	debug_printf("========================================\n\n");
}

